#include <pangolin/pangolin.h>
#include <iostream>

int main(int argc, char **argv)
{
    pangolin:: CreateWindowAndBind("Main", 640, 480);
    
    glEnable(GL_DEPTH_TEST);
    
     pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(640,480,420,420,320,240,0.2,100),
            pangolin::ModelViewLookAt(0,-10,0.1,0,0,0,pangolin::AxisNegY)
    );
    
    pangolin::Handler3D handler(s_cam);
    
    pangolin::View &d_cam=pangolin::CreateDisplay().SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f).SetHandler(&handler);
    
    while(!pangolin::ShouldQuit()){
        glClear(GL_COLOR_BUFFER_BIT| GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        
        pangolin::glDrawAxis(3);
        
        glPointSize(10.0f);
        glBegin(GL_POINTS);
        glColor3f(1.0, 1.0, 1.0);
        glVertex3f(0.0f, 0.0f, 0.0f);
        glVertex3f(1,0,0);
        glVertex3f(0,2,0);
        glEnd();
        
        glPushMatrix();
        std::vector<GLfloat> Twc ={ 1,0,0,0, 0,1,0,0, 0,0,5,1};
        glMultMatrixf(Twc.data());
        
        const float w=2;
        const float h=w*0.75;
        const float z=w*0.6;
        glLineWidth(2);
        glColor3f(1.0,0,0);
        glBegin(GL_LINES);
        
        glVertex3f(0,0,0);
        glVertex3f(w,h,z);
        glVertex3f(0,0,0);
        glVertex3f(w,-h,z);
        glVertex3f(0,0,0);
        glVertex3f(-w, -h,z);
        glVertex3f(0,0,0);
        glVertex3f(-w,h,z);
        glVertex3f(w,h,z);
        glVertex3f(-w,h,z);
        glVertex3f(-w,h,z);
        glVertex3f(-w,-h,z);
        glVertex3f(-w,-h,z);
        glVertex3f(w,-h,z);
        glVertex3f(w,-h,z);
        glVertex3f(w,h,z);
        glEnd();
        
        pangolin::FinishFrame();
    }
    
    return 0;
}
        
        
        
        
